#ifndef IMU_FACTOR_H
#define IMU_FACTOR_H
#include <iostream>
#include <vector>
#include <memory>

#include "imu_preintegration.h"

#include "state.h"

class ImuFactor {
public:  
   ImuFactor(std::shared_ptr<ImuPreIntegration> imu_preintegration);
   void ComputeResidualAndJacobianPoseGraph(const State& last_state, const State& state, Eigen::Matrix<double, 5, 1> &residual, Eigen::Matrix<double, 5, 8> &jacobianXi, Eigen::Matrix<double, 5, 5> &jacobianXj);

   Eigen::Matrix<double, 5, 5> GetCov() {
      return imu_preintegration_->cov_;
   }

private:
   std::shared_ptr<ImuPreIntegration> imu_preintegration_;
};

#endif 
